package estrategia;

import java.io.IOException;
import java.util.ArrayList;
import java.util.Iterator;

import javax.vecmath.Point3d;

import monitor.DetectorAtascamientoD;
import monitor.ManagerObjetos;
import CamposDePotencial.EstrategiaCampoPotencial;
import CamposDePotencial.Objetivo;

import comunicacion.Conector;
import comunicacion.ParametrosEntorno;
import comunicacion.comandos.Commands;
import comunicacion.comandos.EntornoReal;
import comunicacion.simulador.Simulador;


public class Controlador {	
	public Conector entorno;
	
	public Predictor predictor;
	public ManagerObjetos managerObjetos;
	
	private Utiles utiles;
	private DetectorAtascamientoD monitorAtascamiento;
	
	private Deliberador deliberador;
	
	private long instanteInicialPartida;
	
	public boolean puedo_usar_motores = true; //Bandera para mutuo excluir el uso de los motores entre Controlador y DetectorAtascamientoD
	
	
	public Controlador(int entornoConexion) throws IOException {		
		// Inicializamos los parametros de configuracion del entorno
		ParametrosEntorno.cargarPropiedades(entornoConexion);
		
		switch (entornoConexion) {
			case ParametrosEntorno.CONECTAR_CON_SIMULADOR:
				entorno = new Simulador(ParametrosGlobales.simIP, ParametrosGlobales.simPuerto, 0, 1);
				break;
			case ParametrosEntorno.CONECTAR_CON_ENTORNO_REAL:
				entorno = new EntornoReal(ParametrosGlobales.comandosIP, ParametrosGlobales.comandosPuerto, ParametrosGlobales.macRobot, Commands.NXT_MOTOR_A, Commands.NXT_MOTOR_B);
				break;
			default:
				entorno = null;	
		}
		
		managerObjetos = ManagerObjetos.getInstance(entorno);
		
		utiles = new Utiles(entorno);
		predictor = new Predictor(entorno, managerObjetos);
				
		deliberador = new Deliberador(entorno, managerObjetos);
		
		monitorAtascamiento = new DetectorAtascamientoD(this);
		monitorAtascamiento.start();		
	}
	
	public void moverRobotLyapunov(Point3d destino) throws IOException {
		Point3d actual = entorno.getCoordenadas(); 
		double distancia = utiles.distanciaEuclidea(actual, destino);
		
		while (distancia > ParametrosEntorno.DISTANCIA_ACEPTABLE) {			
			double v = utiles.velocidadLinealLV(actual, destino);				
			double w = utiles.velocidadAngularLV(actual, destino);
			
			double vl = v - w*ParametrosGlobales.DISTANCIA_ENTRE_RUEDAS/2.0;
			double vr = v + w*ParametrosGlobales.DISTANCIA_ENTRE_RUEDAS/2.0;
			
			entorno.enviarVelocidades(vl, vr);
			
			actual = entorno.getCoordenadas();
			distancia = utiles.distanciaEuclidea(actual, destino);
			
		}
		
		entorno.detenerMotores();
	}
	
	public void moverRobotDirect(Point3d destino) throws IOException {
		
		Point3d actual = entorno.getCoordenadas(); 
		double distancia = utiles.distanciaEuclidea(actual, destino);
		
		double maxVelocidad = ParametrosEntorno.MAXIMA_VELOCIDAD_DIRECT;
		double factorVariacion = ParametrosEntorno.FACTOR_VARIACION_VELOCIDAD_DIRECT;
		
		while (distancia > ParametrosEntorno.DISTANCIA_ACEPTABLE && puedo_usar_motores) {	
			// Obtenemos el angulo que forma el punto actual con respecto al destino
			double theta = utiles.getAnguloTheta(actual, destino);
			double alpha = utiles.getAnguloAlpha(theta, actual.z);
			
//			double vl, vr;
//			if (alpha < 0) {
//				vl = maxVelocidad;
//				vr = maxVelocidad + factorVariacion*alpha;
//			} else {
//				vl = maxVelocidad - factorVariacion*alpha;
//				vr = maxVelocidad;
//			}
			
			double vl, vr;
			if (Math.abs(alpha) <= Math.PI/2) {
				if (alpha < 0) {
					vl = maxVelocidad;
					vr = maxVelocidad + factorVariacion*alpha;
				} else {
					vl = maxVelocidad - factorVariacion*alpha;
					vr = maxVelocidad;
				}
			} else {
				// Aca agregamos DirectAng
				if (alpha < 0) {
					vl = -maxVelocidad;
					vr = -maxVelocidad + factorVariacion*(Math.PI+alpha);
				} else {
					vl = -maxVelocidad + factorVariacion*(Math.PI-alpha);
					vr = -maxVelocidad;
				}
			}
			
			entorno.enviarVelocidades(vl, vr);
			
			try {
				Thread.sleep(40);
			} catch (InterruptedException e) {}
			
			actual = entorno.getCoordenadas();
			distancia = utiles.distanciaEuclidea(actual, destino);
		}
		
		if( puedo_usar_motores ) {
			entorno.detenerMotores();
		}
	}
	
	public void moverRobotInterceptarEnemigo(int idEnemigo) throws IOException {
		
		Point3d actual = entorno.getCoordenadas();
		Point3d destino = entorno.getCoordenadas(idEnemigo);		
		double distancia = utiles.distanciaEuclidea(actual, destino);		
		double RELACION_DISTANCIA_PASOS_HACIA_FUTURO = 20;
		
		double maxVelocidad = ParametrosEntorno.MAXIMA_VELOCIDAD_DIRECT;
		double factorVariacion = ParametrosEntorno.FACTOR_VARIACION_VELOCIDAD_DIRECT;
		
		while (distancia > 2*ParametrosEntorno.DISTANCIA_ACEPTABLE) {	
			// Obtenemos el angulo que forma el punto actual con respecto al destino
			double theta = utiles.getAnguloTheta(actual, destino);
			double alpha = utiles.getAnguloAlpha(theta, actual.z);
			
			double vl, vr;
			if (alpha < 0) {
				vl = maxVelocidad;
				vr = maxVelocidad + factorVariacion*alpha;
			} else {
				vl = maxVelocidad - factorVariacion*alpha;
				vr = maxVelocidad;
			}
			
			entorno.enviarVelocidades(vl, vr);
			
			try {
				Thread.sleep(40);
			} catch (InterruptedException e) {}
			
			actual = entorno.getCoordenadas();
			Point3d enemigo = entorno.getCoordenadas(idEnemigo);
			distancia = utiles.distanciaEuclidea(actual, enemigo);
			int pasosHaciaFuturo = (int) (distancia * RELACION_DISTANCIA_PASOS_HACIA_FUTURO);
			destino = predictor.predecirPosicion(idEnemigo, pasosHaciaFuturo);		
		}
		
		entorno.detenerMotores();
		System.out.println("Te atrape elemento " + idEnemigo);
	}
	
	public void moverRobotMixto(Point3d destino) throws IOException {
		Point3d actual = entorno.getCoordenadas();
		int cuadrante = utiles.getCuadrante(actual, destino);
		
		double DISTANCIA_LYAPUNOV_DIRECT = 0.5;
		
		switch (cuadrante) {
			case 1: 
				if (destino.x > DISTANCIA_LYAPUNOV_DIRECT && 
						1.79 - destino.y > DISTANCIA_LYAPUNOV_DIRECT) {						
					double margen = 2.0 * DISTANCIA_LYAPUNOV_DIRECT / 3.0;
					Point3d intermedio;
					intermedio = new Point3d(destino.x - margen, destino.y + margen+0.05, 0);
					moverRobotDirect(intermedio);
					moverRobotLyapunov(destino);
				} else {
					moverRobotDirect(destino);
				}							
				break;
			case 4:
				if (destino.x > DISTANCIA_LYAPUNOV_DIRECT &&						
						destino.y > DISTANCIA_LYAPUNOV_DIRECT) {
					double margen = 2.0 * DISTANCIA_LYAPUNOV_DIRECT / 3.0;
					Point3d intermedio;
					intermedio = new Point3d(destino.x - margen, destino.y - margen-0.05, 0);
					moverRobotDirect(intermedio);
					moverRobotLyapunov(destino);
				} else {
					moverRobotDirect(destino);
				}							
				break;
			case 2: case 3:
				moverRobotLyapunov(destino);
		}
	}	
	
	public void elegirPuntoOptimoBorrarEsto() throws IOException {
		Deliberador deliberador = new Deliberador(entorno, managerObjetos);
		
		Point3d punto = deliberador.calcularPuntoOptimo(managerObjetos.getFuertes());
		
		System.out.println("Punto Optimo elegido = (" + punto.x + ", " + punto.y + ", " + punto.z + ")");
	}
	
	public Point3d[] getPosicionesObstaculos() throws IOException {
		
		return entorno.getCoordenadasObjetos();
	}
	
	public Point3d getMiPosicion() throws IOException {
		return entorno.getCoordenadas();
	}
	
	
	/**
	 * Detiene los motores, sin considerar si tiene o no el control de ellos.
	 * */
	public void detenerMotores() throws IOException {
		
		entorno.detenerMotores();
	}
	
	/**
	 * Girar hacia destino, sin considerar si tiene o no el control de los motores
	 * */
	public void girar(Point3d destino) {
		try {
			Point3d actual = entorno.getCoordenadas();
			double theta = utiles.getAnguloTheta(actual, destino);
			
			girar(theta);
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
	}
	
	
	/**
	 * Girar theta, sin considerar si tiene o no el control de los motores
	 * */
	public void girar(double theta) {
	
		if (theta > 2*Math.PI) {
			theta = theta % 2*Math.PI;
		}
		
		if (theta > Math.PI) {
			theta = theta - 2*Math.PI;
		}
			
		try {
			Point3d actual = entorno.getCoordenadas();
		
			double diferencia = utiles.getDiferenciaAngulos(theta, actual.z);
			
			//System.out.println("Diferencia = " + diferencia);
			
			int izq = 1;
			if (theta >=0 && actual.z >=0 || theta <=0 && actual.z <=0) {
				if (theta <= actual.z) {
					izq = 1;
				} else {
					izq = -1;
				}
			} else {
				if (actual.z <= 0 && theta >= 0) {
					if (actual.z + Math.PI >= theta) {
						izq = -1;
					} else {
						izq = 1;
					}
				} else {
					if (actual.z - Math.PI <= theta) {
						izq = 1;
					} else {
						izq = -1;
					}
				}
			}
			
			/*
			while ( diferencia > ParametrosEntorno.TOLERANCIA_ANGULO) {
			//while(!Salird.angulosIguales(theta, actual.z)) {
				entorno.enviarVelocidades(izq*(Math.sqrt(diferencia)), -izq*(Math.sqrt(diferencia)));
				actual = entorno.getCoordenadas();
				diferencia = utiles.getDiferenciaAngulos(theta, actual.z);
				//diferencia = Salird.angulosIguales(theta, actual.z);
				//System.out.println("Diferencia78 = " + diferencia);
			}
			*/
			
			double angulo_barrido = 0;
			double angulo_barrido_total = utiles.getDiferenciaAngulos(theta, actual.z);
			
			//System.out.println("angulo_barrido_total = " + angulo_barrido_total);			
			while(angulo_barrido < angulo_barrido_total - ParametrosEntorno.MARGEN_GIRO  ) {
				double angulo_anterior = actual.z;
				entorno.enviarVelocidades(izq*(Math.sqrt(diferencia)), -izq*(Math.sqrt(diferencia)));
				actual = entorno.getCoordenadas();					
				angulo_barrido = angulo_barrido +  utiles.getDiferenciaAngulos(angulo_anterior, actual.z);
				//System.out.println("angulo_barrido = " + angulo_barrido);
			}
			
			entorno.detenerMotores();
			
		} catch (IOException e) {
			// TODO Auto-generated catch block
			System.out.println("Error al girar");
			e.printStackTrace();
		}
	}
	
	/**
	 * Retrocede a velocidad velocidad, sin controlar si tiene o no el control de los motores
	 * */
	public void retroceder(double velocidad) throws IOException {
		
		entorno.enviarVelocidades(-velocidad, -velocidad);	
		
	}
	
	/**
	 * Avanza a velocidad velocidad, sin controlar si tiene o no el control de los motores
	 * */
	public void avanzar(double velocidad) throws IOException {
		
		entorno.enviarVelocidades(velocidad, velocidad);	
		
	}
	
	public void camposDePotencial(Point3d objetivo) throws IOException {
		
		CamposPotencial camposPotencial = new CamposPotencial();
		Point3d mipos = entorno.getCoordenadas();
		Point3d proxPunto = mipos;
		
		while (utiles.distanciaEuclidea(proxPunto, objetivo) > ParametrosEntorno.DISTANCIA_ACEPTABLE) {
			mipos = entorno.getCoordenadas();
			Point3d[] obstaculos = getPosicionesObstaculos();
			proxPunto = camposPotencial.hallarProximoDestino(mipos, objetivo, obstaculos);
			
			girar(proxPunto);
			moverRobotDirect(proxPunto);		
		}	
	}	
	
	
	public void camposPotencialUnivectorial(Point3d g, Point3d r) throws IOException {
			CamposPotencialUnivectorial campos = new CamposPotencialUnivectorial(utiles);
			
			Point3d p = entorno.getCoordenadas();
			
			double Kp = 20;
			double Kd = 40;
			
			double theta_error_anterior = 0;
			double distancia = utiles.distanciaEuclidea(p, g);
			
			while (distancia > ParametrosEntorno.DISTANCIA_ACEPTABLE) {
			
				Point3d[] obstaculos = entorno.getCoordenadasObjetos();
				Vector resultante = campos.hallarDireccionCampo(p, g, r, obstaculos);
				
				double theta_error = resultante.direccion - p.z; 
				
				double theta_error_der = theta_error - theta_error_anterior;
				theta_error_anterior = theta_error;
				
				double w = Kp*theta_error + Kd*theta_error_der;
				
				double v = utiles.velocidadLinealLV(p, g);
				double Vr = v + (ParametrosGlobales.DISTANCIA_ENTRE_RUEDAS/2)*w;
				double Vl = v - (ParametrosGlobales.DISTANCIA_ENTRE_RUEDAS/2)*w;
				
				entorno.enviarVelocidades(Vl, Vr);
				
				p = entorno.getCoordenadas();
				distancia = utiles.distanciaEuclidea(p, g);
				
			}
			entorno.detenerMotores();
		}
	
	
	// Devuelvo true en caso de que pueda llegar al destino
	public boolean puedoLlegarAlDestino(Objetivo objetivo) throws IOException {
		boolean puedoLlegar = true;
		double distanciaMaxima = ParametrosPotenciales.DISTANCIA_FUERTE;
		// Si el objetivo es un punto y hay un robot cerca entonces devuelvo false
		if (! objetivo.esObjeto()) {
			Point3d posObjetivo = posObjetivo = objetivo.getPosicionObjetivo();;		
			Iterator<Integer> iterObjeto = managerObjetos.getObjetosActivos().iterator();
			
			while(iterObjeto.hasNext()) {
				int idObjeto = iterObjeto.next();
				Point3d posObjeto = predictor.predecirPosicion(idObjeto);
				if (objetivo.getIdObjetivo() != idObjeto) {
					if (utiles.distanciaEuclidea(posObjetivo, posObjeto) < distanciaMaxima) {
						puedoLlegar = false;
						break;
					}
				}
			}
			
		}
		
		return puedoLlegar;
	}
	
	// Realiza la invocacion al control campos de potencial creado un objetivo asociado a un objeto
	public void controlCamposPotencial(int idObjetivo) throws IOException {
		Objetivo objetivo = new Objetivo(idObjetivo, managerObjetos.esFuerte(idObjetivo), predictor);
		controlCamposPotencial(objetivo);
	}
	
	// Realiza la invocacion al control campos de potencial creado un objetivo puntual
	public void controlCamposPotencial(Point3d puntoObjetivo) throws IOException {
		Objetivo objetivo = new Objetivo(puntoObjetivo, predictor);
		controlCamposPotencial(objetivo);
	}
	
	// Control de movimiento asociado a una planificacio nde trayectorias con campos de potenciales
	public void controlCamposPotencial(Objetivo objetivo) throws IOException {
	
		EstrategiaCampoPotencial estrategia = new EstrategiaCampoPotencial(objetivo, managerObjetos.getObjetosActivos(), utiles, entorno, predictor);
		
		double maxVelocidad = ParametrosEntorno.MAXIMA_VELOCIDAD_DIRECT;
		double factorVariacion = ParametrosEntorno.FACTOR_VARIACION_VELOCIDAD_DIRECT;
		double distanciaAceptable = 0;
					
		if (objetivo.esObjeto()) {
			if (managerObjetos.esEjercito(objetivo.getIdObjetivo())) {
				if (managerObjetos.esEnemigo(objetivo.getIdObjetivo()))
					distanciaAceptable = ParametrosPotenciales.DISTANCIA_ENEMIGO;
				else 
					distanciaAceptable = ParametrosPotenciales.DISTANCIA_ALIADO;
			}else 
				distanciaAceptable = ParametrosPotenciales.DISTANCIA_FUERTE;
		} else {
			distanciaAceptable = ParametrosPotenciales.DISTANCIA_PUNTO;
		}
			
		Point3d puntoObjetivo = objetivo.getPosicionObjetivo();
		Point3d mipos = entorno.getCoordenadas();
		double distancia = utiles.distanciaEuclidea(mipos, puntoObjetivo);
		
		// || ! puedoLlegarAlDestino(objetivo)
		// && segundosRestantes() > 0
		while (distancia > distanciaAceptable && puedo_usar_motores && segundosRestantes() > 0) {
			// Le pido a la estrategia el destino dada mi posiciòn actual y el objetivo
			Point3d destino = estrategia.getDestino(mipos, puntoObjetivo);
			
			// Obtenemos el angulo que forma el punto actual con respecto al destino
			double theta = utiles.getAnguloTheta(mipos, destino);
			double alpha = utiles.getAnguloAlpha(theta, mipos.z);
			
			double vl, vr;
			if (alpha < 0) {
				vl = maxVelocidad;
				vr = maxVelocidad + factorVariacion*alpha;
			} else {
				vl = maxVelocidad - factorVariacion*alpha;
				vr = maxVelocidad;
			}
			
			/*double vl, vr;
			if (Math.abs(alpha) <= (Math.PI/2.0)) {
				if (alpha < 0) {
					vl = maxVelocidad;
					vr = maxVelocidad + factorVariacion*alpha;
				} else {
					vl = maxVelocidad - factorVariacion*alpha;
					vr = maxVelocidad;
				}
			} else {
				// Aca agregamos DirectAng
				if (alpha < 0) {
					vl = -maxVelocidad;
					vr = -maxVelocidad + factorVariacion*(Math.PI+alpha);
				} else {
					vl = -maxVelocidad + factorVariacion*(Math.PI-alpha);
					vr = -maxVelocidad;
				}
			}*/
		
			entorno.enviarVelocidades(vl, vr);
			
			try {
				Thread.sleep(40);
			} catch (InterruptedException e) {}
			
			mipos = entorno.getCoordenadas();
			if ( objetivo.esObjeto())
				puntoObjetivo = entorno.getCoordenadas(objetivo.getIdObjetivo());
			else
			    puntoObjetivo =objetivo.getPosicionObjetivo();
			distancia = utiles.distanciaEuclidea(mipos, puntoObjetivo);

		}
		
		if( puedo_usar_motores) {
			entorno.detenerMotores();
		}
		
		
		/*
		if (segundosRestantes() <= 0)
			System.out.println("FIN RECORRIDO POTENCIAL por tiempo");
		if (! puedo_usar_motores)
			System.out.println("FIN RECORRIDO POTENCIAL por atascamiento");
		else 
			System.out.println("FIN RECORRIDO POTENCIAL porque llegue");
		*/			
	}
	
		
	private long segundosRestantes() {
		long transcurrido = System.currentTimeMillis() - instanteInicialPartida;
		return (ParametrosGlobales.TIEMPO_JUEGO - (transcurrido / 1000));
	}
	
	/**
	 * Es el metodo que efectivamente se encarga de ejecutar casi toda la estrategia de juego
	 * La idea es decidir y ejecutar una accion. cuando retoma el control, se repite el ciclo
	 */
	public void jugarConquistaDeFuertes() throws IOException {
		System.out.println("************ NUEVA BATALLA ************");
		System.out.println("Tiempo Total de Juego :: " + ParametrosGlobales.TIEMPO_JUEGO + "segundos");
		System.out.println("************ PARTICIPANTES ************");
		managerObjetos.imprimirClasificacionesActuales();
		
		instanteInicialPartida = System.currentTimeMillis();
			
		while (segundosRestantes() > 0 && DetectorAtascamientoD.robotVivo()) {
			
			System.out.println("-------------------------------------------------");
			
			Point3d miPos = entorno.getCoordenadas();
			// Puede haber un enemigo cerca, acechando, atacando, o simplemente pasando.
			// Para nosotros la cercania se interpreta como amenaza indistintamente
			int enemigoAmenazante = deliberador.enemigoAmenzante(miPos);
			if (enemigoAmenazante >= 0) {
				// No hay mejor defensa que un buen ataque
				// Atacamos al enemigo directamente antes de que el nos ataque a nosotros
				moverRobotHaciaObjetivo(enemigoAmenazante);
			} else if (deliberador.voyGanando()) {
				System.out.println("Voy ganando :: " + segundosRestantes());
				// Buscamos un punto del entorno cercano que permita mejorar un poco
				// por ejemplo dismuyendo la suma de distancias a los fuertes conquistados
				Point3d siguientePunto = deliberador.mejorarPuntoOptimo();
				if (siguientePunto != null) {					
					int objetoCercano = deliberador.objetoCercano(siguientePunto);
					// Si hay un objeto cerca del punto destino lo usamos como referencia
					// En cambio si la zona esta momentaneamente libre, vamos al propio punto
					if (objetoCercano < 0) {
						System.out.println("Corrigiendo un poquito");
						// Aca podemos ir con direct nomas, porque asumimos que no hay nadie cerca
						// y el punto esta al ladito, o sea que no deberia haber problemas
						moverRobotDirect(siguientePunto);
					} else {
						System.out.println("Iba a corregir un poquito pero tengo un objeto cerca, voy a el");
						// Mandamos al robot a acercarse al objeto de destino
						moverRobotHaciaObjetivo(objetoCercano);
					}
				} else {
					System.out.println("Voy ganando y no me sirve moverme :: " + segundosRestantes());
					try {
						Thread.sleep(1000);
					} catch (InterruptedException e) {}		
				}
			} else {
				System.out.println("Empatando o perdiendo");
				// Estoy empatando o perdiendo
				ArrayList<Integer> fuertes = managerObjetos.getFuertes();
				// TODO por ahora asumimos que si no estamos ganando estamos perdiendo
				// Elegimos el siguiente punto al cual ir
				Point3d siguientePunto = deliberador.calcularPuntoOptimo(fuertes);
				
				int cantFuertesHipoteticos = deliberador.getFuertesConquistables(siguientePunto).size();
				// Reviso si aun moviendome al punto "optimo" mi bando sigue perdiendo
				if (cantFuertesHipoteticos < fuertes.size()-cantFuertesHipoteticos) {
					// Busco al enemigo que mas fuertes tiene conquistados al momento
					// y trato de atacarlo a el. En este caso con mi posicion actual y no
					// con la elegida, ya que en igualmente sigo perdiendo
					int mejorEnemigo = deliberador.obtenerMejorEnemigo();
					// TODO aca se supone que siempre va a ser un indice valido, porque si no
					// no podria ir perdiendo, pero por las dudas falta ver que hacer en el else
					if (mejorEnemigo >= 0) {
						System.out.println("DECISION: Atacar al objeto " + mejorEnemigo);
						
						// TODO: Derrapa mucho, debe calibrarse
						//Aumentamos nuestra velocidad para atacar!!
						ParametrosEntorno.MAXIMA_VELOCIDAD_DIRECT = ParametrosEntorno.MAXIMA_VELOCIDAD_DIRECT * 1.5;
						ParametrosEntorno.FACTOR_VARIACION_VELOCIDAD_DIRECT = ParametrosEntorno.FACTOR_VARIACION_VELOCIDAD_DIRECT * 1.5;
						
						
						entorno.enviarBeep();
						moverRobotHaciaObjetivo(mejorEnemigo);
						
						// TODO: Derrapa mucho, debe calibrarse
						//Regresamos la velocidad a su estado normal
						ParametrosEntorno.MAXIMA_VELOCIDAD_DIRECT = ParametrosEntorno.MAXIMA_VELOCIDAD_DIRECT / 1.5;
						ParametrosEntorno.FACTOR_VARIACION_VELOCIDAD_DIRECT = ParametrosEntorno.FACTOR_VARIACION_VELOCIDAD_DIRECT / 1.5;
						
						
					} else {
						System.out.println("Aparentemente voy perdiendo pero tenemos todos los fuertes :S");
					}
				} else {
					System.out.print("DECISION: Ir a punto optimo " + utiles.formatearPunto(siguientePunto));
					// Tratamos de ir hacia el punto "optimo"
					moverRobotHaciaObjetivo(siguientePunto);
					try {
						Thread.sleep(200);
					} catch (InterruptedException e) {}		
					
				}				
			}			
			
		}
		
		// <BORRAR>
		System.out.println("Fin Juego");
		int resultadoFinal = deliberador.getFuertesConquistables(entorno.getCoordenadas()).size();
		int fuertesTotales = managerObjetos.getFuertes().size();
		
		if (fuertesTotales-resultadoFinal < resultadoFinal && DetectorAtascamientoD.robotVivo()) {
			System.out.println("Ganamos " + resultadoFinal + " a " + (fuertesTotales-resultadoFinal));
		} else {
			System.out.println("Perdimos " + (fuertesTotales-resultadoFinal) + " a " + resultadoFinal);
		}
		
		try {
			entorno.enviarBeep();
			Thread.sleep(100);
			entorno.enviarBeep();
		} catch (Exception e) {}
		// </BORRAR>
		
		monitorAtascamiento.killThread();
		
	}
	
	/**
	 * Usa los campos de potenciales para moverse a un punto
	 * @throws IOException 
	 */
	private void moverRobotHaciaObjetivo(Point3d puntoDestino) throws IOException {
		controlCamposPotencial(puntoDestino);
		//moverRobotDirect(puntoDestino);
	}
	
	/**
	 * Usa los campos de potenciales para moverse a un objeto particular
	 * Deberia resolver el problema de detectar si es un fuerte, un enemigo o un aliado
	 * de forma transparente a la estrategia
	 * @throws IOException 
	 */
	private void moverRobotHaciaObjetivo(int objetoDestino) throws IOException {
		controlCamposPotencial(objetoDestino);
		//moverRobotDirect(entorno.getCoordenadas(objetoDestino));
	}
	
}